"""
IMU传感器服务器
提供机器人IMU传感器相关的MCP工具
"""

from typing import Dict, Any
from mcp.server.fastmcp import FastMCP
from sensors.imu_sensor import IMUSensor


class IMUServer(FastMCP):
    """IMU传感器服务器类"""

    def __init__(self):
        """初始化IMU传感器服务器"""
        super().__init__("imu_server")

        # 初始化IMU传感器
        self.imu = IMUSensor(0x79)  # 默认I2C地址0x79

        # 注册工具
        self._register_tools()

    def _register_tools(self):
        """注册所有IMU传感器工具"""

        @self.tool()
        async def get_accel_data(g: bool = False) -> Dict[str, Any]:
            """获取加速度计数据

            Args:
                g: 是否以重力加速度g为单位

            Returns:
                Dict[str, Any]: 加速度计数据
            """
            try:
                accel_data = self.imu.get_accel_data(g)
                return {
                    "status": "success",
                    "data": accel_data,
                    "unit": "g" if g else "m/s²",
                }
            except Exception as e:
                return {"status": "error", "message": str(e)}

        @self.tool()
        async def get_gyro_data() -> Dict[str, Any]:
            """获取陀螺仪数据

            Returns:
                Dict[str, Any]: 陀螺仪数据
            """
            try:
                gyro_data = self.imu.get_gyro_data()
                return {"status": "success", "data": gyro_data, "unit": "°/s"}
            except Exception as e:
                return {"status": "error", "message": str(e)}

        @self.tool()
        async def get_temp() -> Dict[str, Any]:
            """获取温度数据

            Returns:
                Dict[str, Any]: 温度数据
            """
            try:
                temp = self.imu.get_temp()
                return {"status": "success", "temperature": temp, "unit": "°C"}
            except Exception as e:
                return {"status": "error", "message": str(e)}

        @self.tool()
        async def get_all_data() -> Dict[str, Any]:
            """获取所有传感器数据

            Returns:
                Dict[str, Any]: 所有传感器数据
            """
            try:
                accel_data = self.imu.get_accel_data()
                gyro_data = self.imu.get_gyro_data()
                temp = self.imu.get_temp()

                return {
                    "status": "success",
                    "data": {
                        "accel": {"data": accel_data, "unit": "m/s²"},
                        "gyro": {"data": gyro_data, "unit": "°/s"},
                        "temp": {"data": temp, "unit": "°C"},
                    },
                }
            except Exception as e:
                return {"status": "error", "message": str(e)}

        @self.tool()
        async def set_accel_range(accel_range: int) -> Dict[str, Any]:
            """设置加速度计量程

            Args:
                accel_range: 量程值(2,4,8,16)

            Returns:
                Dict[str, Any]: 执行结果
            """
            try:
                if accel_range == 2:
                    self.imu.set_accel_range(self.imu.ACCEL_RANGE_2G)
                elif accel_range == 4:
                    self.imu.set_accel_range(self.imu.ACCEL_RANGE_4G)
                elif accel_range == 8:
                    self.imu.set_accel_range(self.imu.ACCEL_RANGE_8G)
                elif accel_range == 16:
                    self.imu.set_accel_range(self.imu.ACCEL_RANGE_16G)
                else:
                    return {
                        "status": "error",
                        "message": f"Invalid accelerometer range: {accel_range}",
                    }

                return {
                    "status": "success",
                    "message": f"Set accelerometer range to {accel_range}G",
                }
            except Exception as e:
                return {"status": "error", "message": str(e)}

        @self.tool()
        async def set_gyro_range(gyro_range: int) -> Dict[str, Any]:
            """设置陀螺仪量程

            Args:
                gyro_range: 量程值(250,500,1000,2000)

            Returns:
                Dict[str, Any]: 执行结果
            """
            try:
                if gyro_range == 250:
                    self.imu.set_gyro_range(self.imu.GYRO_RANGE_250DEG)
                elif gyro_range == 500:
                    self.imu.set_gyro_range(self.imu.GYRO_RANGE_500DEG)
                elif gyro_range == 1000:
                    self.imu.set_gyro_range(self.imu.GYRO_RANGE_1000DEG)
                elif gyro_range == 2000:
                    self.imu.set_gyro_range(self.imu.GYRO_RANGE_2000DEG)
                else:
                    return {
                        "status": "error",
                        "message": f"Invalid gyroscope range: {gyro_range}",
                    }

                return {
                    "status": "success",
                    "message": f"Set gyroscope range to {gyro_range}°/s",
                }
            except Exception as e:
                return {"status": "error", "message": str(e)}

        @self.tool()
        async def get_accel_range() -> Dict[str, Any]:
            """获取当前加速度计量程

            Returns:
                Dict[str, Any]: 量程信息
            """
            try:
                accel_range = self.imu.read_accel_range()
                return {"status": "success", "range": accel_range, "unit": "G"}
            except Exception as e:
                return {"status": "error", "message": str(e)}

        @self.tool()
        async def get_gyro_range() -> Dict[str, Any]:
            """获取当前陀螺仪量程

            Returns:
                Dict[str, Any]: 量程信息
            """
            try:
                gyro_range = self.imu.read_gyro_range()
                return {"status": "success", "range": gyro_range, "unit": "°/s"}
            except Exception as e:
                return {"status": "error", "message": str(e)}
